﻿using System;
using System.Diagnostics;
using System.Runtime.InteropServices;
//using Mono.Simd;

using Num = System.Single;

namespace MicoBenchmarks
{
	//[StructLayout(LayoutKind.Sequential)]
	[StructLayout(LayoutKind.Explicit)] 
	struct Vector4
	{
		//public float X, Y, Z, W;
		[FieldOffset(0)] public Num X;
		[FieldOffset(4)] public Num Y;
		[FieldOffset(8)] public Num Z;
		[FieldOffset(12)] public Num W;
		
		//[FieldOffset(0)] private Vector4f vector;

		public Vector4(Num x, Num y, Num z, Num w)
		{
			X = x;
			Y = y;
			Z = z;
			W = w;
		}

		public override string ToString()
		{
			return string.Format("{0}, {1}, {2}, {3}", X, Y, Z, W);
		}
		
		public static Vector4 operator + (Vector4 a, Vector4 b)
		{
			//a.vector += b.vector;
			//return a;

			return new Vector4
			(
				a.X + b.X,
				a.Y + b.Y,
				a.Z + b.Z,
				a.W + b.W
			);
		}
		
		public static Vector4 operator - (Vector4 a, Vector4 b)
		{
			//a.vector -= b.vector;
			//return a;

			return new Vector4
			(
				a.X - b.X,
				a.Y - b.Y,
				a.Z - b.Z,
				a.W - b.W
			);
		}
	}
	/*
	//[StructLayout(LayoutKind.Sequential)]
	[StructLayout(LayoutKind.Explicit)] 
	struct Matrix4
	{
		[FieldOffset(0)] public Vector4 X;
		[FieldOffset(16)] public Vector4 Y;
		[FieldOffset(32)] public Vector4 Z;
		[FieldOffset(48)] public Vector4 W;
		
		[FieldOffset(0)] public Vector4f R0;
		[FieldOffset(16)] public Vector4f R1;
		[FieldOffset(32)] public Vector4f R2;
		[FieldOffset(48)] public Vector4f R3;

		public Matrix4(Vector4 x, Vector4 y, Vector4 z, Vector4 w)
		{
			X = x;
			Y = y;
			Z = z;
			W = w;
		}

		public Matrix4 Multiply(Matrix4 matrix)
		{
			return new Matrix4
			(
				new Vector4((matrix.X.X*X.X) + (matrix.X.Y*Y.X) + (matrix.X.Z*Z.X) + (matrix.X.W*W.X), (matrix.X.X*X.Y) + (matrix.X.Y*Y.Y) + (matrix.X.Z*Z.Y) + (matrix.X.W*W.Y), (matrix.X.X*X.Z) + (matrix.X.Y*Y.Z) + (matrix.X.Z*Z.Z) + (matrix.X.W*W.Z), (matrix.X.X*X.W) + (matrix.X.Y*Y.W) + (matrix.X.Z*Z.W) + (matrix.X.W*W.W)),
				new Vector4((matrix.Y.X*X.X) + (matrix.Y.Y*Y.X) + (matrix.Y.Z*Z.X) + (matrix.Y.W*W.X), (matrix.Y.X*X.Y) + (matrix.Y.Y*Y.Y) + (matrix.Y.Z*Z.Y) + (matrix.Y.W*W.Y), (matrix.Y.X*X.Z) + (matrix.Y.Y*Y.Z) + (matrix.Y.Z*Z.Z) + (matrix.Y.W*W.Z), (matrix.Y.X*X.W) + (matrix.Y.Y*Y.W) + (matrix.Y.Z*Z.W) + (matrix.Y.W*W.W)),
				new Vector4((matrix.Z.X*X.X) + (matrix.Z.Y*Y.X) + (matrix.Z.Z*Z.X) + (matrix.Z.W*W.X), (matrix.Z.X*X.Y) + (matrix.Z.Y*Y.Y) + (matrix.Z.Z*Z.Y) + (matrix.Z.W*W.Y), (matrix.Z.X*X.Z) + (matrix.Z.Y*Y.Z) + (matrix.Z.Z*Z.Z) + (matrix.Z.W*W.Z), (matrix.Z.X*X.W) + (matrix.Z.Y*Y.W) + (matrix.Z.Z*Z.W) + (matrix.Z.W*W.W)),
				new Vector4((matrix.W.X*X.X) + (matrix.W.Y*Y.X) + (matrix.W.Z*Z.X) + (matrix.W.W*W.X), (matrix.W.X*X.Y) + (matrix.W.Y*Y.Y) + (matrix.W.Z*Z.Y) + (matrix.W.W*W.Y), (matrix.W.X*X.Z) + (matrix.W.Y*Y.Z) + (matrix.W.Z*Z.Z) + (matrix.W.W*W.Z), (matrix.W.X*X.W) + (matrix.W.Y*Y.W) + (matrix.W.Z*Z.W) + (matrix.W.W*W.W))
			);
		}
		
		public Matrix4 Multiply2(Matrix4 matrix)
		{
			Vector4f t1, t2;
			Vector4f out0, out1, out2, out3;
			
			t1 = (R0.Shuffle(ShuffleSel.ExpandX) * matrix.R0) + (R0.Shuffle(ShuffleSel.ExpandY) * matrix.R1);
			t2 = (R0.Shuffle(ShuffleSel.ExpandZ) * matrix.R2) + (R0.Shuffle(ShuffleSel.ExpandW) * matrix.R3);
			out0 = t1 + t2;
			
			t1 = (R1.Shuffle(ShuffleSel.ExpandX) * matrix.R0) + (R1.Shuffle(ShuffleSel.ExpandY) * matrix.R1);
			t2 = (R1.Shuffle(ShuffleSel.ExpandZ) * matrix.R2) + (R1.Shuffle(ShuffleSel.ExpandW) * matrix.R3);
			out1 = t1 + t2;
			
			t1 = (R2.Shuffle(ShuffleSel.ExpandX) * matrix.R0) + (R2.Shuffle(ShuffleSel.ExpandY) * matrix.R1);
			t2 = (R2.Shuffle(ShuffleSel.ExpandZ) * matrix.R2) + (R2.Shuffle(ShuffleSel.ExpandW) * matrix.R3);
			out2 = t1 + t2;
			
			t1 = (R3.Shuffle(ShuffleSel.ExpandX) * matrix.R0) + (R3.Shuffle(ShuffleSel.ExpandY) * matrix.R1);
			t2 = (R3.Shuffle(ShuffleSel.ExpandZ) * matrix.R2) + (R3.Shuffle(ShuffleSel.ExpandW) * matrix.R3);
		
			return new Matrix4
			{
				R0 = out0,
				R1 = out1,
				R2 = out2,
				R3 = out3
			};
		}
		
		public Matrix4 Multiply2(ref Matrix4 matrix)
		{
			return new Matrix4
			{
				R0 =
					(R0.Shuffle(ShuffleSel.ExpandX) * matrix.R0) + (R0.Shuffle(ShuffleSel.ExpandY) * matrix.R1) +
					(R0.Shuffle(ShuffleSel.ExpandZ) * matrix.R2) + (R0.Shuffle(ShuffleSel.ExpandW) * matrix.R3),
				R1 =
					(R1.Shuffle(ShuffleSel.ExpandX) * matrix.R0) + (R1.Shuffle(ShuffleSel.ExpandY) * matrix.R1) +
					(R1.Shuffle(ShuffleSel.ExpandZ) * matrix.R2) + (R1.Shuffle(ShuffleSel.ExpandW) * matrix.R3),
				R2 =
					(R2.Shuffle(ShuffleSel.ExpandX) * matrix.R0) + (R2.Shuffle(ShuffleSel.ExpandY) * matrix.R1) +
					(R2.Shuffle(ShuffleSel.ExpandZ) * matrix.R2) + (R2.Shuffle(ShuffleSel.ExpandW) * matrix.R3),
				R3 =
					(R3.Shuffle(ShuffleSel.ExpandX) * matrix.R0) + (R3.Shuffle(ShuffleSel.ExpandY) * matrix.R1) +
					(R3.Shuffle(ShuffleSel.ExpandZ) * matrix.R2) + (R3.Shuffle(ShuffleSel.ExpandW) * matrix.R3)
			};
		}

		public Matrix4 RotateAroundAxisX(float radians)
		{
			float tCos = (float)Math.Cos(-radians), tSin = (float)Math.Sin(-radians);
			return new Matrix4
			(
				X,
				new Vector4((Y.X*tCos) - (Z.X*tSin), (Y.Y*tCos) - (Z.Y*tSin), (Y.Z*tCos) - (Z.Z*tSin), 0),
				new Vector4((Y.X*tSin) + (Z.X*tCos), (Y.Y*tSin) + (Z.Y*tCos), (Y.Z*tSin) + (Z.Z*tCos), 0),
				W
			);
		}

		public Matrix4 RotateAroundAxisY(float radians)
		{
			float tCos = (float)Math.Cos(-radians), tSin = (float)Math.Sin(-radians);
			return new Matrix4
			(
				new Vector4((Z.X*tSin) + (X.X*tCos), (Z.Y*tSin) + (X.Y*tCos), (Z.Z*tSin) + (X.Z*tCos), 0),
				Y,
				new Vector4((Z.X*tCos) - (X.X*tSin), (Z.Y*tCos) - (X.Y*tSin), (Z.Z*tCos) - (X.Z*tSin), 0),
				W
			);
		}

		public Matrix4 RotateAroundAxisZ(float radians)
		{
			float tCos = (float)Math.Cos(-radians), tSin = (float)Math.Sin(-radians);
			return new Matrix4
			(
				new Vector4((X.X*tCos) - (Y.X*tSin), (X.Y*tCos) - (Y.Y*tSin), (X.Z*tCos) - (Y.Z*tSin), 0),
				new Vector4((X.X*tSin) + (Y.X*tCos), (X.Y*tSin) + (Y.Y*tCos), (X.Z*tSin) + (Y.Z*tCos), 0),
				Z,
				W
			);
		}

		public override string ToString()
		{
			return string.Format("{0} : {1} : {2} : {3}", X, Y, Z, W);
		}
	}
	*/
	static class Matricies
	{
		public static void RunMultiply(Stopwatch stopwatch, float randomValue)
		{
			long mil = 0;
			/*
			var matrix = new Matrix4
			(
				new Vector4(1, 0, 0, 0),
				new Vector4(0, 1, 0, 0),
				new Vector4(0, 0, 1, 0),
				new Vector4(0, 0, 0, 1)
			);
			
			var matrix2 = new Matrix4
			(
				new Vector4(1, 0, 0, 0),
				new Vector4(0, 1, 0, 0),
				new Vector4(0, 0, 1, 0),
				new Vector4(0, 0, 0, 1)
			);
			
			matrix = matrix.RotateAroundAxisX(randomValue);
			matrix = matrix.RotateAroundAxisY(randomValue);
			matrix = matrix.RotateAroundAxisZ(randomValue);
			
			matrix2 = matrix2.RotateAroundAxisX(randomValue * 1.1f);
			matrix2 = matrix2.RotateAroundAxisY(randomValue * 1.2f);
			matrix2 = matrix2.RotateAroundAxisZ(randomValue * 1.4f);
			*/
			
			var a = new Vector4(randomValue, randomValue+1, randomValue+2, randomValue+3);
			var b = new Vector4(1, 1, 1, 1);
			
			for (int test = 0; test != 100; ++test)
			{
				stopwatch.Start();
				
				for (uint i = 0; i != 1000000u; ++i)
				{
					//matrix = matrix.Multiply2(ref matrix2);
					a += b;
					b -= a;
				}
				
				stopwatch.Stop();
				mil += stopwatch.ElapsedMilliseconds;
				stopwatch.Reset();
			}
			
			//Console.WriteLine(string.Format("RunStructTest: Value={0}, Value2={1}, Mil={2}", matrix, matrix2, mil));
			Console.WriteLine("{0}, {1}, {2}, {3}", a.X, a.Y, a.Z, a.W);
			Console.WriteLine("{0}, {1}, {2}, {3}", b.X, b.Y, b.Z, b.W);
			Console.WriteLine(string.Format("RunStructTest: Mil={0}", (double)mil / 100));
		}
	}
}
